using BepuUtilities;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Collidables
{
    /// <summary>
    /// 用于增量生成复合形状的可重复使用的便捷型。
    /// </summary>
    public struct CompoundBuilder : IDisposable
    {
        public BufferPool Pool;
        public Shapes Shapes;

        public struct Child
        {
            public RigidPose LocalPose;
            public TypedIndex ShapeIndex;
            /// <summary>
            /// 与此孩子关联的体重。在解释为动态复合时充当孩子的质量。
            /// 当解释为具有重新定位的运动学时,它将用作计算旋转中心的局部姿势权重。
            /// </summary>
            public float Weight;
            /// <summary>
            /// 与子项关联的惯性张量。如果惯性全为零,则解释为无穷大。
            /// </summary>
            public Symmetric3x3 Inertia;
        }

        public QuickList<Child> Children;

        public CompoundBuilder(BufferPool pool, Shapes shapes, int builderCapacity)
        {
            Pool = pool;
            Shapes = shapes;
            Children = new QuickList<Child>(builderCapacity, Pool);
        }

        /// <summary>
        /// 向累加器添加新形状,从而在形状集中创建新形状。用于计算惯性张量的质量将基于给定的重量。
        /// </summary>
        /// <typeparam name="TShape">要添加到累加器和形状集的形状的类型。</typeparam>
        /// <param name="Shape">要添加的形状。</param>
        /// <param name="localPose">形状在复合的本地空间中的姿势。</param>
        /// 形状的<param name="weight">权重。如果化合物被解释为动力学,这将被用作质量,并标度惯性张量。
        /// 否则,用于重新定位。</param>
        public void Add<TShape>(in TShape shape, in RigidPose localPose, float weight) where TShape : unmanaged, IConvexShape
        {
            ref var child = ref Children.Allocate(Pool);
            child.LocalPose = localPose;
            child.ShapeIndex = Shapes.Add(shape);
            child.Weight = weight;
            shape.ComputeInertia(weight, out var inertia);
            Symmetric3x3.Invert(inertia.InverseInertiaTensor, out child.Inertia);
        }

        /// <summary>
        /// 向累加器添加新形状,从而在形状集中创建新形状。惯性假定为无穷大。
        /// </summary>
        /// <typeparam name="TShape">要添加到累加器和形状集的形状的类型。</typeparam>
        /// <param name="Shape">要添加的形状。</param>
        /// <param name="localPose">形状在复合的本地空间中的姿势。</param>
        /// 形状的<param name="weight">权重。如果该化合物被解释为动力学,则这将被用作质量。否则,用于重新定位。</param>
        public void AddForKinematic<TShape>(in TShape shape, in RigidPose localPose, float weight) where TShape : unmanaged, IConvexShape
        {
            ref var child = ref Children.Allocate(Pool);
            child.LocalPose = localPose;
            child.ShapeIndex = Shapes.Add(shape);
            child.Weight = weight;
            child.Inertia = default;
        }

        /// <summary>
        /// 向累加器添加新形状。
        /// </summary>
        /// <param name="Shape">要添加的形状的索引。</param>
        /// <param name="localPose">形状在复合的本地空间中的姿势。</param>
        /// 形状的<param name="weight">权重。如果该化合物被解释为动力学,则这将被用作质量。否则,用于重新定位。</param>
        /// <param name="inverseInertia">要添加的形状的逆惯性张量。假定已根据权重进行了所需的缩放。</param>
        public void Add(TypedIndex shape, in RigidPose localPose, in Symmetric3x3 inverseInertia, float weight)
        {
            Debug.Assert(Compound.ValidateChildIndex(shape, Shapes));
            ref var child = ref Children.Allocate(Pool);
            child.LocalPose = localPose;
            child.ShapeIndex = shape;
            child.Weight = weight;
            // 这假设给定的惯性是非奇异的。这应该是一个有效的假设,除非用户试图提供轴锁定张量。
            // 对于这样的用例,最好在计算"正常"惯性后锁定轴。
            Debug.Assert(Symmetric3x3.Determinant(inverseInertia) > 0,
                "Shape inertia tensors should be invertible. If making an axis-locked compound, consider locking the axis on the completed inertia. " +
                "If making a kinematic, consider using the overload which takes no inverse inertia.");
            Symmetric3x3.Invert(inverseInertia, out child.Inertia);
        }

        /// <summary>
        /// 假定累加器具有无限惯性,则向累加器添加新形状。
        /// </summary>
        /// <param name="Shape">要添加的形状的索引。</param>
        /// <param name="localPose">形状在复合的本地空间中的姿势。</param>
        /// <param name="weight">用于计算旋转中心的形状的权重。</param>
        public void AddForKinematic(TypedIndex shape, in RigidPose localPose, float weight)
        {
            Debug.Assert(Compound.ValidateChildIndex(shape, Shapes));
            ref var child = ref Children.Allocate(Pool);
            child.LocalPose = localPose;
            child.ShapeIndex = shape;
            child.Weight = weight;
            child.Inertia = default;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetOffsetInertiaContribution(in Vector3 offset, float mass, out Symmetric3x3 contribution)
        {
            var innerProduct = Vector3.Dot(offset, offset);
            contribution.XX = mass * (innerProduct - offset.X * offset.X);
            contribution.YX = -mass * (offset.Y * offset.X);
            contribution.YY = mass * (innerProduct - offset.Y * offset.Y);
            contribution.ZX = -mass * (offset.Z * offset.X);
            contribution.ZY = -mass * (offset.Z * offset.Y);
            contribution.ZZ = mass * (innerProduct - offset.Z * offset.Z);
        }

        /// <summary>
        /// 从动态复合的累加集中构建复合子对象的缓冲区。
        /// 计算质心,并相对于质心将子形状重新居中。不重置累加器。
        /// </summary>
        /// <param name="Children">从累计集创建的子项的列表。</param>
        /// <param name="惯性">化合物的组合惯性。</param>
        /// <param name="center">根据累加子对象的姿势和权重计算旋转中心。</param>
        public void BuildDynamicCompound(out Buffer<CompoundChild> children, out BodyInertia inertia, out Vector3 center)
        {
            center = new Vector3();
            float totalWeight = 0;
            for (int i = 0; i < Children.Count; ++i)
            {
                center += Children[i].LocalPose.Position * Children[i].Weight;
                totalWeight += Children[i].Weight;
            }
            Debug.Assert(totalWeight > 0, "The compound as a whole must have nonzero weight when using a recentering build. The center is undefined.");

            inertia.InverseMass = 1f / totalWeight;
            center *= inertia.InverseMass;
            Pool.Take(Children.Count, out children);
            Symmetric3x3 summedInertia = default;
            for (int i = 0; i < Children.Count; ++i)
            {
                ref var sourceChild = ref Children[i];
                ref var targetChild = ref children[i];
                targetChild.LocalPose.Position = sourceChild.LocalPose.Position - center;
                GetOffsetInertiaContribution(targetChild.LocalPose.Position, sourceChild.Weight, out var contribution);
                Symmetric3x3.Add(contribution, summedInertia, out summedInertia);
                Symmetric3x3.Add(summedInertia, sourceChild.Inertia, out summedInertia);
                targetChild.LocalPose.Orientation = sourceChild.LocalPose.Orientation;
                targetChild.ShapeIndex = sourceChild.ShapeIndex;
            }
            Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor);
        }

        /// <summary>
        /// 从动态复合的累加集中构建复合子对象的缓冲区。不会让孩子们重新居中。不重置累加器。
        /// </summary>
        /// <param name="Children">从累计集创建的子项的列表。</param>
        /// <param name="惯性">化合物的组合惯性。</param>
        public void BuildDynamicCompound(out Buffer<CompoundChild> children, out BodyInertia inertia)
        {
            float totalWeight = 0;
            for (int i = 0; i < Children.Count; ++i)
            {
                totalWeight += Children[i].Weight;
            }
            Debug.Assert(totalWeight > 0, "The compound as a whole must have nonzero weight when creating a dynamic compound.");

            inertia.InverseMass = 1f / totalWeight;
            Pool.Take(Children.Count, out children);
            Symmetric3x3 summedInertia = default;
            for (int i = 0; i < Children.Count; ++i)
            {
                ref var sourceChild = ref Children[i];
                ref var targetChild = ref children[i];
                targetChild.LocalPose.Position = sourceChild.LocalPose.Position;
                GetOffsetInertiaContribution(targetChild.LocalPose.Position, sourceChild.Weight, out var contribution);
                Symmetric3x3.Add(contribution, summedInertia, out summedInertia);
                Symmetric3x3.Add(summedInertia, sourceChild.Inertia, out summedInertia);
                targetChild.LocalPose.Orientation = sourceChild.LocalPose.Orientation;
                targetChild.ShapeIndex = sourceChild.ShapeIndex;
            }
            Symmetric3x3.Invert(summedInertia, out inertia.InverseInertiaTensor);
        }

        /// <summary>
        /// 从运动学复合的累加集中构建复合子对象的缓冲区。
        /// 计算质心,并相对于质心将子形状重新居中。不重置累加器。
        /// </summary>
        /// <param name="Children">从累计集创建的子项的列表。</param>
        /// <param name="惯性">化合物的组合惯性。</param>
        /// <param name="center">根据累加子对象的姿势和权重计算旋转中心。</param>
        public void BuildKinematicCompound(out Buffer<CompoundChild> children, out Vector3 center)
        {
            center = new Vector3();
            float totalWeight = 0;
            for (int i = 0; i < Children.Count; ++i)
            {
                center += Children[i].LocalPose.Position * Children[i].Weight;
                totalWeight += Children[i].Weight;
            }
            Debug.Assert(totalWeight > 0, "The compound as a whole must have nonzero weight when using a recentering build. The center is undefined.");

            var inverseWeight = 1f / totalWeight;
            center *= inverseWeight;
            Pool.Take(Children.Count, out children);
            for (int i = 0; i < Children.Count; ++i)
            {
                ref var sourceChild = ref Children[i];
                ref var targetChild = ref children[i];
                targetChild.LocalPose.Position = sourceChild.LocalPose.Position - center;
                targetChild.LocalPose.Orientation = sourceChild.LocalPose.Orientation;
                targetChild.ShapeIndex = sourceChild.ShapeIndex;
            }
        }

        /// <summary>
        /// 从运动学复合的累加集中构建复合子对象的缓冲区。不会使孩子重新居中。不重置累加器。
        /// </summary>
        /// <param name="Children">从累计集创建的子项的列表。</param>
        /// <param name="惯性">化合物的组合惯性。</param>
        /// <param name="center">根据累加子对象的姿势和权重计算旋转中心。</param>
        public void BuildKinematicCompound(out Buffer<CompoundChild> children)
        {
            Pool.Take(Children.Count, out children);
            for (int i = 0; i < Children.Count; ++i)
            {
                ref var sourceChild = ref Children[i];
                ref var targetChild = ref children[i];
                targetChild.LocalPose.Position = sourceChild.LocalPose.Position;
                targetChild.LocalPose.Orientation = sourceChild.LocalPose.Orientation;
                targetChild.ShapeIndex = sourceChild.ShapeIndex;
            }
        }

        /// <summary>
        /// 清空累积的子项。
        /// </summary>
        public void Reset()
        {
            Children.Count = 0;
        }

        /// <summary>
        /// 将内部资源返回到池,从而使生成器不可用。
        /// </summary>
        public void Dispose()
        {
            Children.Dispose(Pool);
        }
    }
}
